#include <WProgram.h>
#include <Servo.h>
#include <Wire.h>
#include <rfm70.h>
#include "ROB_Telemetry.h"
#include "ROB_Console.h"
#include "ROB_Led.h"
#include "ROB_i2cBusMaster.h"
#include "ROB_Motor.h"
#include "ROB_Pantilt.h"
//#include "ROB_IrRC.h"
#include "ROB_AppleRC.h"
#include "ROB_RC.h"
#include "ROB_Sensorshield.h"
#include "rfm70.h"
#include "ROB_RFM70.h"


//////////////////////////////////////////////////////////////////////
// Pin declarations
//////////////////////////////////////////////////////////////////////

//#define PIN_IR_REMOTE 2 // IR input low active
#define PIN_LED 8
#define PIN_SERVO_PAN A0
#define PIN_SERVO_TILT A1
#define PIN_RFM_CE 9



unsigned long actualMillis;
unsigned long fastLoopLastMillis;
unsigned long mediumLoopLastMillis;
unsigned long slowLoopLastMillis;
uint16_t fastLoopDelay;
uint16_t mediumLoopDelay;
uint16_t slowLoopDelay;

///////////////////////////////////////////////////////////
// setup
///////////////////////////////////////////////////////////


void setup() {
  Led.Begin(PIN_LED);  
  Led.on();

  Console.Begin();
  Telemetry.Begin();
  Pantilt.Begin(PIN_SERVO_PAN, PIN_SERVO_TILT);
  I2cBusmaster.Begin();
  Sensorshield.Begin();
  Motor.Begin();  
  RC.Begin();
  AppleRC.Begin();
  RFM70RC.Begin(PIN_RFM_CE);

  Led.off();


  fastLoopLastMillis = 0;
  mediumLoopLastMillis = 0;
  slowLoopLastMillis = 0;

  fastLoopDelay = 5;
  mediumLoopDelay = 100;
  slowLoopDelay = 1000;

}


///////////////////////////////////////////////////////////
// loop
///////////////////////////////////////////////////////////


void loop() {

  actualMillis = millis();

  // load calculation

  // loopslow
  if(actualMillis - slowLoopLastMillis > slowLoopDelay) {
    Motor.requestSensors();
    Console.update();
    slowLoopLastMillis = actualMillis;
  }

  // loopmed
  if(actualMillis - mediumLoopLastMillis > mediumLoopDelay) {
    Sensorshield.requestSensors();
    mediumLoopLastMillis = actualMillis;
  }


  // loopfast
  if(actualMillis - fastLoopLastMillis > fastLoopDelay) {
    if(I2cBusmaster.update() == I2C_ERROR_TIMEOUT)
      I2cBusmaster.reset();
    Motor.update();
    Pantilt.update();
    AppleRC.update();
    RC.update();
    Sensorshield.update();
    RFM70RC.update();
   
    Led.update();
    fastLoopLastMillis = actualMillis;
  }

}



// ##TODOS
// favs looprunner
// cavs looprunner
// mocs looprunner
// cam looprunner
// telemetry load and save
// chuck button handler


